% clear
clc
%% park transform
%park matrix cos
syms a
assume(a*3/pi,'integer')
Tcosf1t_a = zeros(3,3);
for i=2:3
    Tcosf1t_a(i-1,i)=0.5;
    Tcosf1t_a(i,i-1)=0.5;
end
Tcosf1t_a = sym(Tcosf1t_a);
Tcosf1t_b = Tcosf1t_a;
for i=2:3
    Tcosf1t_b(i-1,i) = Tcosf1t_a(i-1,i)*exp(1i*a);
    Tcosf1t_b(i,i-1) = Tcosf1t_a(i,i-1)*exp(-1i*a);
end
Tcosf1t_c = Tcosf1t_a;
for i=2:3
    Tcosf1t_c(i-1,i) = Tcosf1t_a(i-1,i)*exp(-1i*a);
    Tcosf1t_c(i,i-1) = Tcosf1t_a(i,i-1)*exp(1i*a);
end
%park matrix sin
Tsinf1t_a = zeros(3,3);
for i=2:3
    Tsinf1t_a(i-1,i)=1i*0.5;
    Tsinf1t_a(i,i-1)=(-1i*0.5);
end
Tsinf1t_a = sym(Tsinf1t_a);
Tsinf1t_b = Tsinf1t_a;
for i=2:3
    Tsinf1t_b(i-1,i) = Tsinf1t_a(i-1,i)*exp(1i*a);
    Tsinf1t_b(i,i-1) = Tsinf1t_a(i,i-1)*exp(-1i*a);
end
Tsinf1t_c = Tsinf1t_a;
for i=2:3
    Tsinf1t_c(i-1,i) = Tsinf1t_a(i-1,i)*exp(-1i*a);
    Tsinf1t_c(i,i-1) = Tsinf1t_a(i,i-1)*exp(1i*a);
end

%%
% Gv is voltage sampling
syms Gv_sn1 Gv_s0 Gv_s1
assume([Gv_sn1,Gv_s0,Gv_s1])
GvB = [Gv_sn1,Gv_s0,Gv_s1];
TGv = diag(GvB);
% Gi is current sampling
syms Gi_sn1 Gi_s0 Gi_s1
assume([Gi_sn1,Gi_s0,Gi_s1])
GiB = [Gi_sn1,Gi_s0,Gi_s1];
TGi = diag(GiB);
%% forward matrix G
%transfor function of output filter, seen from converter side voltage to current in outer filter;
%  ^^^^(current)^^^....$$$(voltage)$$$$
%                   ..
%                   ^^
%                   ==     (^resistance =capacitor $inductance)
syms L1_s0 %TL(s-0*jw1)
syms L1_s1 %TL(s-1jw1)
syms L1_sn1 %TL(s+1jw1)
assume([L1_sn1,L1_s0,L1_s1],'real')
L1B = [L1_sn1,L1_s0,L1_s1];
G = -inv(diag(L1B));
vpa(G,4);
%% input filter for passive filter
syms Kf_s0 %TL(s-0*jw1)
syms Kf_s1 %TL(s-1jw1)
syms Kf_sn1 %TL(s+1jw1)
assume([Kf_sn1,Kf_s0,Kf_s1],'real')
KfB = [Kf_sn1,Kf_s0,Kf_s1];
Kf = (diag(KfB));
vpa(Kf,4);
%% back matrix H
% H is current feedback path without effect of PLL
vec_atob = diag([exp(1i*a),1,exp(-1i*a)]);
vec_atoc = diag([exp(-1i*a),1,exp(1i*a)]);

syms Gv50
assume(Gv50,'real');
Gv50B = [inv(Gv50),1,Gv50];
invGv50B = [Gv50,1,inv(Gv50)];
TGv50 = diag(Gv50B);
TinvGv50 = diag(invGv50B);

park_d = 2/3*(Tcosf1t_a+Tcosf1t_b*vec_atob+Tcosf1t_c*vec_atoc);
park_q = -2/3*(Tsinf1t_a+Tsinf1t_b*vec_atob+Tsinf1t_c*vec_atoc);

%transfor function of output filter, L = s*L;
syms H_s0 %TL(s-0*jw1)
syms H_s1 %TL(s-1jw1)
syms H_sn1 %TL(s+1jw1)
assume([H_sn1,H_s0,H_s1],'real')
HB = [H_sn1,H_s0,H_s1];
TH = diag(HB);
syms Kdq
assume(Kdq,'real')
Tkdq = diag(Kdq*ones(1,3));
syms Vdc
assume(Vdc,'real')
syms Km_sn1 Km_s0 Km_s1
assume([Km_sn1,Km_s0,Km_s1],'real')
KmB = [Km_sn1,Km_s0,Km_s1];
Km = diag(KmB);
TVdc = diag(Vdc*ones(1,3));


Hi = (Tcosf1t_a*(-TH*park_d-Tkdq*park_q)-Tsinf1t_a*(-TH*park_q+Tkdq*park_d))*TGi;
H = Km*TVdc*TGv50*Hi*TinvGv50;
% intermediate variable for display
Hi_disp = sym('Hi_disp',[3 3]).*nonzero_mask(Hi);
H_disp = subs(H,Hi,Hi_disp);
H = simplify(H);


% R1 is pll effect path join at entry of input port of current controller
%transfor function of pll, theta = Tpll*vqs;
syms Tpll_s0 %Tpll(s-0*jw1)
syms Tpll_s1 %Tpll(s-1jw1)
syms Tpll_sn1 %Tpll(s+1jw1)
assume([Tpll_sn1,Tpll_s0,Tpll_s1],'real')
syms I1d I1q D1d D1q
assume([I1d,I1q,D1d,D1q],'real')
TI1d = diag(I1d*ones(1,3));
TI1q = diag(I1q*ones(1,3));
TI1 = TI1d+TI1q*1i;
% % intermediate variable for display
% TI1_disp = sym('TI1_disp',[3 3]);
TD1d = diag(D1d*ones(1,3));
TD1q = diag(D1q*ones(1,3));
% % intermediate variable for display
% TD1_disp = sym('TD1_disp',[3 3]);

TpllB = [Tpll_sn1,Tpll_s0,Tpll_s1];
TTpll = diag(TpllB);

Rpll = (Tcosf1t_a*(TI1q*(-TH)-TI1d*(-Tkdq))-Tsinf1t_a*(-TI1d*(-TH)+TI1q*Tkdq))*TTpll*park_q...
    +(-TD1q*Tcosf1t_a-TD1d*Tsinf1t_a)*TTpll*park_q;
R = Km*TVdc*TGv50*Rpll*TinvGv50*TGv;
% intermediate variable for display
Rpll_disp = sym('Rpll_disp',[3 3]).*nonzero_mask(Rpll);
R_disp = subs(R,Rpll,Rpll_disp);
R = simplify(R);


%% close loop
Yden = (diag(ones(1,3))+G*H);
Yden = subs(Yden,a,2*pi/3);
Ynum = G*(Kf*diag(ones(1,3))-R);
Ynum = subs(Ynum,a,2*pi/3);
Y = -inv(Yden)*Ynum

Yden_disp = (diag(ones(1,3))+G*H_disp);
Yden_disp = subs(Yden_disp,a,2*pi/3); 
vpa(Yden,4);
Ynum_disp = G*(Kf*diag(ones(1,3))-R_disp);
Ynum_disp = subs(Ynum_disp,a,2*pi/3);
vpa(Ynum,4);
Y_disp = -inv(Yden_disp)*Ynum_disp
%vpa(inv_Fcls_num*inv(inv_Fcls_den),2)




fp = fopen('Ydisp.txt','w');
fprintf(fp,'%s \n',latex(Y_disp));
fprintf(fp,'%s \n',latex(subs(Hi,a,2*pi/3)));
fprintf(fp,'%s \n',latex(subs(Rpll,a,2*pi/3)));
fclose(fp);
save admit_load.mat Y TGi TGv Km TTpll TH G GiB GvB KmB TpllB HB L1B KfB D1d D1q I1d I1q Kdq Vdc Gv50

